/**
  ******************************************************************************
  * @file    crg.c
  * @author  hyseim software Team
  * @date    01-Sep-2023
  * @brief   This file provides all the crg functions.
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2020 Hyseim. Co., Ltd.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "common.h"
#include "crg.h"

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/

CRG_Clk_t sysClk = {
    .pllClk = 0,
    .cpuClk = 12000000,
    .busClk = 12000000,
    .perClk = 12000000,
};

static inline void CRG_ClkNop(void)
{
    asm volatile (
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
        "nop\n\t"
    );
}

// 0 --- pll clk
// 1 --- ref clk
// 2 --- otgphy pll480m clk
void CRG_SelSysClkSrc(CRG_SysClk_t clkSel)
{
    int clkSrcSel = 0;

    if (clkSel > 2) {
        clkSrcSel = 0;
    }
    else {
        clkSrcSel = clkSel;
    }

    CRG->CFG_CRG_REG000 = clkSrcSel << 7 | (0x3 << (16 + 7)); //cfg_crg_reg000[ 8: 7]:cfg_sel_pll_bypass
    CRG_ClkNop();
}

void CRG_Pll_output_bypass(CRG_FuncEnable_t bypassEnable)
{
    CRG->CFG_CRG_REG000 = bypassEnable << 2 | (1 << (16 + 2)) ;
    CRG_ClkNop();
}

void CRG_PllEnable(CRG_FuncEnable_t pllEnable)
{
  //CRG->CFG_CRG_REG000 = pllEnable << 0 | (1 << (16 + 0)) ;
    reg32(CRG_AO_BASE+0x0) = 0 << 6 | (1 << (16 + 6)) ; // set pll ready to 0
    CRG->CFG_CRG_REG000 = pllEnable << 0 | (1 << (16 + 0)) |
                                      0 << 0 | (1 << (16 + 2)) ; // pll bypass
    CRG_ClkNop();
}

void CRG_PllCfgUpdate(CRG_FuncEnable_t pllCfgUpdate)
{
    CRG->CFG_CRG_REG000 = pllCfgUpdate << 1 | (1 << (16 + 1)) ;
    CRG_ClkNop();
}

void CRG_SetPllFreq(CRG_PllInit_t CRG_PllInitStruct)
{
    uint32_t regdata;

    //CRG_Pll_output_bypass(FUNC_ENABLE);
    CRG_PllEnable(FUNC_DISABLE); //
    //regdata = CRG->CFG_CRG_REG00C);
    //
    CRG->CFG_CRG_REG004 = CRG_PllInitStruct.pllFbkInt |
                            (CRG_PllInitStruct.pllPreDiv << 10) |
                            (CRG_PllInitStruct.pllPostDiv << 12);
    CRG->CFG_CRG_REG008 = CRG_PllInitStruct.pllFbkFra;

    CRG_PllCfgUpdate(FUNC_ENABLE);
    CRG_PllEnable(FUNC_ENABLE);
    // wait pll lock.
    do {
        regdata = CRG->CFG_CRG_REG00C;
    }while ((regdata & 0x01) == 0);

    CRG_PllCfgUpdate(FUNC_DISABLE);
    CRG_Pll_output_bypass(FUNC_DISABLE);
}

void CRG_SetPllClk600mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x64;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x01;

    CRG_SetPllFreq(CRG_PllInitStruct);
}
void CRG_SetPllClk500mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x54;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x01;

    CRG_SetPllFreq(CRG_PllInitStruct);
}
void CRG_SetPllClk400mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x85;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x02;

    CRG_SetPllFreq(CRG_PllInitStruct);
}
void CRG_SetPllClk350mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x75;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x02;

    CRG_SetPllFreq(CRG_PllInitStruct);
}
void CRG_SetPllClk300mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x64;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x02;

    CRG_SetPllFreq(CRG_PllInitStruct);
}

void CRG_SetPllClk275mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x5b;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x02;

    CRG_SetPllFreq(CRG_PllInitStruct);
}

void CRG_SetPllClk250mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x54;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x02;

    CRG_SetPllFreq(CRG_PllInitStruct);
}

void CRG_SetPllClk200mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x85;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x03;

    CRG_SetPllFreq(CRG_PllInitStruct);
}
void CRG_SetPllClk180mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x78;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x03;

    CRG_SetPllFreq(CRG_PllInitStruct);
}

void CRG_SetPllClk150mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x64;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x03;

    CRG_SetPllFreq(CRG_PllInitStruct);
}


void CRG_SetPllClk120mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0xA0;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x04;

    CRG_SetPllFreq(CRG_PllInitStruct);
}

void CRG_SetPllClk100mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x85;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x04;

    CRG_SetPllFreq(CRG_PllInitStruct);
}

void CRG_SetPllClk75mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x64;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x04;

    CRG_SetPllFreq(CRG_PllInitStruct);
}
void CRG_SetPllClk50mhz(void)
{
    CRG_PllInit_t CRG_PllInitStruct;

    CRG_PllInitStruct.pllFbkFra  = 0x0;
    CRG_PllInitStruct.pllFbkInt  = 0x85;
    CRG_PllInitStruct.pllPreDiv  = 0x0;
    CRG_PllInitStruct.pllPostDiv = 0x05;

    CRG_SetPllFreq(CRG_PllInitStruct);
}

void CRG_SysClkDiv(uint32_t cpudiv,uint32_t l2cdiv,uint32_t busdiv)
{
    CRG->CFG_CRG_REG020 = (busdiv << 4) | (0x7 << (16 + 4))|
                      //  (l2cdiv << 8) | (0x7 << (16 + 8))|
                          (cpudiv << 0) | (0xf << (16 + 0));
}

void CRG_DivCpuBusPerClk(uint32_t cpudiv,uint32_t busdiv,uint32_t perdiv)
{
    uint32_t reg020 = (perdiv << 4) | (0x7 << (16 + 4)) |
                      (busdiv << 8) | (0x7 << (16 + 8)) |
                      (cpudiv << 0) | (0xf << (16 + 0));

    //printf("reg020 = 0x%x %d %d %d\r\n",reg020, cpudiv, busdiv, perdiv);
    CRG->CFG_CRG_REG020 = reg020;
    CRG_ClkNop();
}

//cfg_crg_reg020 [ 6: 4], cfg_div_clk_bus
void CRG_Cpu2busClkDiv(uint32_t clkDiv)
{
    CRG->CFG_CRG_REG020 = clkDiv << 4 | (0x7 << (16 + 4)) ;
}

//cfg_crg_reg020 [ 3: 0], cfg_div_clk_cpu
void CRG_Main2cpuClkDiv(uint32_t clkDiv)
{
    CRG->CFG_CRG_REG020 = clkDiv << 0 | (0xf << (16 + 0));
}

//cfg_crg_reg064[11:0], cfg_div_motor_iclk
void CRG_Main2motorClkDiv(uint32_t clkDiv)
{
    CRG->CFG_CRG_REG064 = clkDiv << 0 | (0xfff << (16 + 0));
}

void set_sysclk_500m_busclk_166m(void)
{
    CRG_SelSysClkSrc(REF_CLK);
    CRG_Main2motorClkDiv(CLK_DIV3);
    CRG_Main2cpuClkDiv(CLK_DIV1);
    CRG_Cpu2busClkDiv(CLK_DIV3);
    CRG_SetPllClk500mhz();
    CRG_SelSysClkSrc(PLL_CLK);
}

void set_sysclk_300m_busclk_150m(void)
{
    CRG_SelSysClkSrc(REF_CLK);
    CRG_Main2motorClkDiv(CLK_DIV4);
    CRG_Main2cpuClkDiv(CLK_DIV2);
    CRG_Cpu2busClkDiv(CLK_DIV2);
    CRG_SetPllClk600mhz();
    CRG_SelSysClkSrc(PLL_CLK);
}

void set_sysclk_200m_busclk_100m(void)
{
    CRG_SelSysClkSrc(REF_CLK);
    CRG_Main2motorClkDiv(CLK_DIV4);
    CRG_Main2cpuClkDiv(CLK_DIV2);
    CRG_Cpu2busClkDiv(CLK_DIV2);
    CRG_SetPllClk400mhz();
    CRG_SelSysClkSrc(PLL_CLK);
}

void set_sysclk_400m_busclk_200m(void)
{
    CRG_SelSysClkSrc(REF_CLK);
    CRG_Main2motorClkDiv(CLK_DIV4);
    CRG_Main2cpuClkDiv(CLK_DIV1);
    CRG_Cpu2busClkDiv(CLK_DIV2);
    CRG_SetPllClk400mhz();
    CRG_SelSysClkSrc(PLL_CLK);
}
void set_sysclk_200m_busclk_200m(void)
{
    CRG_SelSysClkSrc(REF_CLK);
    CRG_Main2motorClkDiv(CLK_DIV1);
    CRG_Main2cpuClkDiv(CLK_DIV1);
    CRG_Cpu2busClkDiv(CLK_DIV1);
    CRG_SetPllClk200mhz();
    CRG_SelSysClkSrc(PLL_CLK);
}
void set_sysclk_150m_busclk_150m(void)
{
    CRG_SelSysClkSrc(REF_CLK);
    CRG_Main2motorClkDiv(CLK_DIV1);
    CRG_Main2cpuClkDiv(CLK_DIV1);
    CRG_Cpu2busClkDiv(CLK_DIV1);
    CRG_SetPllClk150mhz();
    CRG_SelSysClkSrc(PLL_CLK);
}

void set_sysclk_120m_busclk_60m(void)
{
    CRG_SelSysClkSrc(REF_CLK);
    CRG_Main2motorClkDiv(CLK_DIV1);
    CRG_Main2cpuClkDiv(CLK_DIV1);
    CRG_Cpu2busClkDiv(CLK_DIV2);
    CRG_SetPllClk120mhz();
    CRG_SelSysClkSrc(PLL_CLK);
}
void set_sysclk_180m_busclk_180m(void)
{
    CRG_SelSysClkSrc(REF_CLK);
    CRG_Main2motorClkDiv(CLK_DIV1);
    CRG_Main2cpuClkDiv(CLK_DIV1);
    CRG_Cpu2busClkDiv(CLK_DIV1);
    CRG_SetPllClk180mhz();
    CRG_SelSysClkSrc(PLL_CLK);
}
void set_sysclk_refclk_busclk_5m(void)
{
    CRG_SelSysClkSrc(REF_CLK);
    CRG_Cpu2busClkDiv(CLK_DIV2);
}

void set_sysclk_otg480m_busclk_160m(void)
{
  CRG_SelSysClkSrc(REF_CLK);
  CRG_Cpu2busClkDiv(CLK_DIV3);
  CRG_SelSysClkSrc(OTG480_CLK);
}

void CRG_SetCpuBusPerClk(uint32_t clkPll, uint32_t cpuDiv, uint32_t busDiv, uint32_t perDiv)
{
    CRG_SelSysClkSrc(REF_CLK);
    CRG_DivCpuBusPerClk(cpuDiv, busDiv, perDiv);
    switch (clkPll){
        case 50:
            CRG_SetPllClk50mhz();
            break;
        case 75:
            CRG_SetPllClk75mhz();
            break;
        case 100:
            CRG_SetPllClk100mhz();
            break;
        case 120:
            CRG_SetPllClk120mhz();
            break;
        case 150:
            CRG_SetPllClk150mhz();
            break;
        case 180:
            CRG_SetPllClk180mhz();
            break;
        case 200:
            CRG_SetPllClk200mhz();
            break;
        case 250:
            CRG_SetPllClk250mhz();
            break;
        case 275:
            CRG_SetPllClk275mhz();
            break;
        case 300:
            CRG_SetPllClk300mhz();
            break;
        case 350:
            CRG_SetPllClk350mhz();
            break;
        case 400:
            CRG_SetPllClk400mhz();
            break;
        case 500:
            CRG_SetPllClk500mhz();
            break;
        case 600:
            CRG_SetPllClk600mhz();
            break;
        default:
            break;
    }
    CRG_SelSysClkSrc(PLL_CLK);

    sysClk.pllClk = clkPll * 1000000;
    sysClk.cpuClk = sysClk.pllClk;
    while (cpuDiv) {
        sysClk.cpuClk = (sysClk.cpuClk >> 1);
        cpuDiv--;
    }
    sysClk.busClk = sysClk.cpuClk;
    while (busDiv) {
        sysClk.busClk = (sysClk.busClk >> 1);
        busDiv--;
    }
    sysClk.perClk = sysClk.busClk;
    while (perDiv) {
        sysClk.perClk = (sysClk.perClk >> 1);
        perDiv--;
    }
}

uint32_t CRG_SetPllClk ( float pll_freq)
{
  CRG_PllInit_t CRG_PllInitStruct;
  uint32_t int_ok = 0;
  unsigned int  fvco;
  unsigned int  fpfd      = 12;
  unsigned int freq_int  = 0;
  unsigned int freq_frac = 0;
  unsigned int freq_out  = 0;
  unsigned int fbk_fra  = 0;
  unsigned int fbk_int  = 0;
  unsigned int post_div = 0;

  CRG_PllInitStruct.pllPreDiv  = 0x0 ;//@12mhz ref pre_div must eq 0
  printf("pll adjusting...\n");

  // 1000 < fvco < 2500
  freq_int = pll_freq*10;
  for(post_div = 0 ; post_div < 4; post_div ++ ) {
    for(fbk_int = 1 ; fbk_int < 512; fbk_int ++ ) {
      fvco = fpfd * (float)fbk_int;
      //printf("\n%d,%d,%d,%d,%d --> ",fbk_int,post_div,fpfd,fvco,freq_int);
      if ((fvco < 1000) || (fvco > 2500)) continue;
      freq_out = fvco * 10 / (1<<post_div);
      //printf("%d <-----", freq_out);
      if ( (freq_out > (freq_int-fpfd)) && (freq_out <= freq_int)) {
        int_ok = 1;
        break;
      }
    }
    if ( int_ok ) {
      //printf("LoL 0\n");
      break;
    }
  }
  if ( !int_ok ) {
    //printf("LoL 2\n");
    return 1;
  }
  freq_frac = freq_int - freq_out;
  //printf("freq_frac %d\n",freq_frac );
  fbk_fra = (((freq_frac * (1<<24)* (1<<post_div))/10)/fpfd) ;
  printf("freq fbk_fra  %d\n",fbk_fra );
  printf("freq fbk_int  %d\n",fbk_int );
  printf("freq post_div %d\n",post_div);

  if ( fbk_fra >= (1<<24) ) {
    printf("LoL 3\n");
    return 2;
  }
  printf("freq set to %f\n",12.0*(fbk_int + (fbk_fra/(1<<24)))/(1<<post_div));

  CRG_PllInitStruct.pllFbkFra  = fbk_fra ;
  CRG_PllInitStruct.pllFbkInt  = fbk_int ;
  CRG_PllInitStruct.pllPostDiv = post_div;

  CRG_SetPllFreq(CRG_PllInitStruct);
  return 0;
}

void set_sys_clk (float pll_freq,uint32_t cpudiv, uint32_t l2cdiv, uint32_t busdiv)
{
    CRG_SelSysClkSrc(REF_CLK);
    CRG_SysClkDiv(cpudiv,l2cdiv,busdiv);
    CRG_SetPllClk(pll_freq);
    CRG_SelSysClkSrc(PLL_CLK);
}
void CRG_ClkTest(uint32_t clkPll, uint32_t cpuDiv, uint32_t busDiv, uint32_t perDiv)
{
    uint32_t clk_cpu = cpuDiv ? clkPll / 2 : clkPll;
    uint32_t clk_bus = busDiv ? clk_cpu / 2 : clk_cpu;
    uint32_t clk_per = perDiv ? clk_bus / 2 : clk_bus;
    CRG_SetCpuBusPerClk(clkPll, cpuDiv, busDiv, perDiv);
    printf("\r\npll clk %dM\r\n",clkPll);
    printf("cpu_clk %dM bus_clk %dM per clk %dM 1\r\n",clk_cpu, clk_bus, clk_per);
    printf("cpu_clk %dM bus_clk %dM per clk %dM 2\r\n",clk_cpu, clk_bus, clk_per);
    printf("cpu_clk %dM bus_clk %dM per clk %dM 3\r\n",clk_cpu, clk_bus, clk_per);

}
